//      exists.cpp
//
//      Copyright 2012 Stefan Fruhner <stefan.fruhner@gmail.com>
//
//      This program is free software; you can redistribute it and/or modify
//      it under the terms of the GNU General Public License as published by
//      the Free Software Foundation; either version 2 of the License, or
//      (at your option) any later version.
//
//      This program is distributed in the hope that it will be useful,
//      but WITHOUT ANY WARRANTY; without even the implied warranty of
//      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//      GNU General Public License for more details.
//
//      You should have received a copy of the GNU General Public License
//      along with this program; if not, write to the Free Software
//      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
//      MA 02110-1301, USA.


#include <iostream>
#include <mylibs/mymath.hpp>
#include <mylibs/cmdline.hpp>
#include <mylibs/mystring.hpp>
#include <mylibs/cmdline.hpp>
#include <mylibs/myinifiles.hpp>
#include <mylibs/mymesh.hpp>
#include <mylibs/myio.hpp>
#include <mylibs/units.hpp>
#include <Magick++.h>

using namespace myMath;
using namespace std;
using namespace mylibs;
using namespace mylibs::cmdline;
using namespace mylibs::IO;

#define INFO(name) info(name, #name)

bool debug = false;

void SetupTransformationMatrix( Matrix &M,
								const vector<double> &Position,
								const vector<double> &Orientation,
								const vector<double> &PixelSpacing){
	M(0,0) = Orientation[0] * PixelSpacing[0];
	M(1,0) = Orientation[1] * PixelSpacing[0];
	M(2,0) = Orientation[2] * PixelSpacing[0];
	M(3,0) = 0.;

	M(0,1) = Orientation[3] * PixelSpacing[1];
	M(1,1) = Orientation[4] * PixelSpacing[1];
	M(2,1) = Orientation[5] * PixelSpacing[1];
	M(3,1) = 0.;

	//! \todo PixelSpacing[2] cannot be read from a single images of an image stack

	/// in this case: PixelSpacing[2] == 1.!
	M(0,2) = (Orientation[1] * Orientation[5] - Orientation[2] * Orientation[4])  * PixelSpacing[2];
	M(1,2) = (Orientation[2] * Orientation[3] - Orientation[0] * Orientation[5])  * PixelSpacing[2];
	M(2,2) = (Orientation[0] * Orientation[4] - Orientation[1] * Orientation[3])  * PixelSpacing[2];
	M(3,2) = 0.;

	M(0,3) = Position[0];
	M(1,3) = Position[1];
	M(2,3) = Position[2];
	M(3,3) = 1.;
}

void SetupTransformationMatrix( Matrix &M,
								const vector<double> &Position,
								const vector<double> &Orientation){
	M(0,0) = Orientation[0];
	M(1,0) = Orientation[1];
	M(2,0) = Orientation[2];
	M(3,0) = 0.;

	M(0,1) = Orientation[3];
	M(1,1) = Orientation[4];
	M(2,1) = Orientation[5];
	M(3,1) = 0.;

	M(0,2) = (Orientation[1] * Orientation[5] - Orientation[2] * Orientation[4]);
	M(1,2) = (Orientation[2] * Orientation[3] - Orientation[0] * Orientation[5]);
	M(2,2) = (Orientation[0] * Orientation[4] - Orientation[1] * Orientation[3]);
	M(3,2) = 0.;

	M(0,3) = Position[0];
	M(1,3) = Position[1];
	M(2,3) = Position[2];
	M(3,3) = 1.;
}

class Transformation {
	public:
		enum Action {Action_None, Action_Remap, Action_Translate, Action_Rotate, Action_Scale};

		Transformation(string a, mystring input_unit) :
									action(Action_None),
									target_unit(input_unit),
									unit(input_unit),
									Factor(1.),
									checked(false) {
			values.clear();
			cout << " --> new trafo : " << a << endl;
			if (a == "remap") 		{
				action =  Action_Remap;
				remap.push_back(0); // no remapping
				remap.push_back(1);
				remap.push_back(2);
			}
			else
			if (a == "translate") {
				action =  Action_Translate;
				Translate = Point(); // actually no translation
			}
			else
			if (a == "rotate") 	{
				action =  Action_Rotate;
				Rotate.assign(3,3,0.);
				Rotate(0,0) = 1.;	// actually no rotation
				Rotate(1,1) = 1.;
				Rotate(2,2) = 1.;
			}
			else
			if (a == "scale") 	{
				action =  Action_Scale;
			}
		}
		const Action& type() {return action;}
		void read_dicom(const mystring &image){
			Magick::Image dcm(image.c_str());

			mystring tmp = "none";
			tmp	 = dcm.attribute(string("dcm:ImagePosition(Patient)"));
			vector<double> Position = tmp.extract<double>();

			tmp = dcm.attribute(string("dcm:ImageOrientation(Patient)"));
			vector<double> Orientation = tmp.extract<double>();

			Rotate.clear();
			Rotate.assign(3, 3, 0.);

			Rotate(0,0) = Orientation[0];
			Rotate(1,0) = Orientation[1];
			Rotate(2,0) = Orientation[2];
			Rotate(0,1) = Orientation[3];
			Rotate(1,1) = Orientation[4];
			Rotate(2,1) = Orientation[5];
			Rotate(0,2) = (Orientation[1] * Orientation[5] - Orientation[2] * Orientation[4]);
			Rotate(1,2) = (Orientation[2] * Orientation[3] - Orientation[0] * Orientation[5]);
			Rotate(2,2) = (Orientation[0] * Orientation[4] - Orientation[1] * Orientation[3]);

//			Rotate.info   ("Rotate ");
			Translate.set(Position);
//			Translate.info("Transl.");
		}

		bool add_rule(mystring word){

			if (word.startswith("unit:")){ // if a unit was given
				unit = word(word.find(":")+1).strip();
				// compute the factor that is needed to convert the
				// transformation into the unit of the input data.

				Factor = mylibs::units::SI_Prefix_Factor(target_unit, unit);
				cout << "factor ["<< target_unit<<"/"<<unit<<"] = " << Factor << endl;
				return true;
			}

			switch (action){
				case Action_None:
					throw myexception("No action defined. What the hell is goin' on ? ");
					break;
				case Action_Remap:
					if (word.contains("=")){
						vector<mystring> l = word.splitv("=");
						if (l.size() != 2) return false;
						if (l[0].size() == 0) return false;
						l[1].replace("x", "0");
						l[1].replace("y", "1");
						l[1].replace("z", "2");
						int val = l[1].ToInt();
						switch (l[0][0]){
							case 'x': remap[0] = val;	break;
							case 'y': remap[1] = val;	break;
							case 'z': remap[2] = val;	break;
						}
						return true;
					}
					break;
				case Action_Translate:{
						if (word.startswith("file:")) { // if to load from dicom image
							values.clear();
							filename = word(4);
							if (filename.file_ext().lower() == "dcm")
								read_dicom(filename);
							else
								throw myexception("Filetype"+filename.file_ext()+ " not supported");
							Translate.info("Translate");
							return true;
						} else if (word == "reverse") {
							Rotate.transpose();
							Vector R = Rotate * Vector(Translate);
							Translate.set(R);
							Translate *= -1.;
							Translate.info("Translate");
							return true;
						}
						// otherwise normal values
						double v = 0.; word >> v; 	// convert to double
						values.push_back(v);	  	// stack value
						Translate.set(values); 		// update translation vector
						return true;
					}
					break;
				case Action_Rotate:{
						if (word.startswith("file:")) {
							values.clear();
							filename = word(4);
							if (filename.file_ext().lower() == "dcm")
								read_dicom(filename);
							else
								throw myexception("Filetype"+filename.file_ext()+ " not supported");
							return true;
						} else if (word == "reverse") {
							Rotate.transpose();
							Vector R = Rotate * Vector(Translate);
							Translate.set(R);
							Translate *= -1.;
							return true;
						}

					}
					break;
				case Action_Scale:{
						word >> Factor;
						return true;
					}
					break;
			}
			return false;
		}

		bool apply(Point &p){

			check(); // maintenance

			switch (action){
				case Action_None:
					cerr << "Empty action" << endl;
					return false;
				case Action_Remap:{
						Point t(p);
						for (size_t i = 0; i < 3; i++) p[i] = t[std::abs(remap[i])];
						for (size_t i = 0; i < 3; i++)
							if (remap[i] < 0 ) p[i] *= -1.;
						return true;
					}
				case Action_Translate:{
						p += Translate;
						return true;
					}
				case Action_Rotate:{
						Vector R = Rotate * Vector(p); 	// do the rotation
						p.set(R);
						p += Translate;					// apply Translation
						return true;
					}
				case Action_Scale:{
						p *= Factor;
						return true;
					}
			}
			return false;
		}

		bool apply_rotations(Point &p){ // for vectors describing directions
										// only directional changes are applied

			check();
			switch (action){
				case Action_None:
					cerr << "Empty action" << endl;
					return false;
				case Action_Remap:{
						Point t(p);
						for (size_t i = 0; i < 3; i++) p[i] = t[std::abs(remap[i])];
						for (size_t i = 0; i < 3; i++)
							if (remap[i] < 0 ) p[i] *= -1.;
						return true;
					}
				case Action_Translate:{
						// ignore
						return true;
					}
				case Action_Rotate:{
						Vector R = Rotate * Vector(p); 	// do the rotation
						p.set(R);
						// no translation
						return true;
					}
				case Action_Scale:{
						// ignore
						return true;
					}
			}
			return false;
		}

		static bool is_action(mystring word){
			return ( 	(word == "remap")
					or  (word == "translate")
					or  (word == "rotate")
					or  (word == "scale")
					);
		}
	private:
		bool check(){ // initializer function that needs to be called befor appliing a tranformation
			if (checked) return false; // nothing to be done

			// do everything necessary
			Translate *= Factor; // convert to the correct unit
			if (action == Action_Scale) Factor = mylibs::units::SI_Prefix_Factor(unit, target_unit);
			checked = true;
			return checked;
		}

		Action action;
		mystring  target_unit;	// unit of the input data
		mystring		 unit;	// unit of the transformation data
		double	 	   Factor; 	// factor that shall be appplied before doing the transformation
		vector<int> 	remap;	// rules for remapping
		Point 		Translate;	// rules for translation
		Matrix		   Rotate;	// rotation matrix
		vector<double> values;	// temporary values
		mystring 	 filename;  // possibly involved filenames
		bool 		  checked;	// If the method check() needs to be run.
};

vector<Transformation> Transformations;
mystring input = mystring();
mystring input_unit = mystring("m");

void read_transformations(const char *filename){
	Transformations.clear();

	fstream f(filename);
	mystring word;
	bool section_input = false;
	do {
		f >> word;
		if (f.fail()) break;
		if (word.startswith("#")) { // ignore comments
			getline(f, word);
			continue;
		}

		// parse input section
		if (word == "input") section_input = true;
		if (section_input){
			if (word.startswith("file:")){
				input = word(word.find(":")+1).strip();
				TELL(input);
				continue;
			};

			if (word.startswith("unit:")){
				input_unit = word(word.find(":")+1).strip();
				TELL(input_unit);
				continue;
			}
		}

		if (Transformation::is_action(word)) {
			section_input=false;
			Transformations.push_back(Transformation(word, input_unit));
			continue;
		}

		if (Transformations.size() == 0) continue;

		Transformation &t = Transformations.back();
		t.add_rule(word);
		if ( debug ) cout << word << endl;
	} while (not f.fail());
	f.close();
}

void generate_template(){
	ofstream o("template.transformation");

	o << "#### Template input file for 'transform'.\n\
## Apply transformations on a set of vectors. Possible\
 transformations are remapping, scaling, rotation, translation.\n\
## Unit conversions are performed automatically if the units are known\n\
\n\
#### The input section must be always present, defining the data to be\n\
###  transformed. *.pts and *.vec(t) files can be used as input.\n\
##\n\
	input \n\
		file:input.pts\n\
		unit:mm\n\
## translate all vectors by a constant vector\n\
	translate\n\
		0, 0, 10\n\
		unit:cm\n\
## remap coordinate system\n\
#	remap \n\
#		x=x\n\
#		y=-z\n\
#		z=y\n\
# Apply a rotation matrix\n\
#	rotate\n\
#		# rotate according to dicom coords\n\
#		dcm:IM_0334.dcm\n\
##		If we want to transform into the image coordinates. That means\n\
##		reverse transformation with respect to the values in the\n\
##		dicom header.\n\
#		reverse\n\
##	translate by the offset given in a dicom header file\n\
#	translate\n\
#		dcm:IM_0334.dcm\n\
##	scale by a constant factor\n\
#	scale\n\
#		1000.\n\
#		unit:cm\n\
" << endl;
	o.close();
	cmdline::warning(" File template.transformation written. Please re-run with -t <file>.transformation.");
}

int main(int argc, char **argv){

	myIniFiles ini(argc, argv);

	ini.set_info("Apply transformations on a set of vectors. Possible\
 transformations are remapping, scaling, rotation, translation. These can be\
 defined in a textfile. Example: \n\
	input \n\
		file:input.pts\n\
		unit:mm\n\
	remap \n\
		x=x\n\
		y=-z\n\
		z=y\n\
	translate\n\
		+5.159800, -98.44, -8.394502\n\
	rotate\n\
		# rotate according to dicom coords\n\
		dcm:IM_0334.dcm\n\
		# If we want to transform into the image coordinates. That means\n\
		# reverse transformation with respect to the values in the\n\
		# dicom header.\n\
		reverse\n\
	translate\n\
		dcm:IM_0334.dcm\n\
	scale\n\
		1000.\n\
		unit:cm\n\
");

	ini.register_flag("verbose", "v", "Verbose information");
//	ini.register_param("image", "i", "Dicom image to load.");
	ini.register_param("output", "o", "Filename to write.");
	ini.register_param("transformation", "t", "File that describes all tranformations to be applied.");
//	ini.register_param("vec"  , "V", "File name of a *.vec file containing vectors in a cartesian coordinate system.");

	debug = ini.exists("verbose");

	mystring output 			= ini.read("output", mystring());
	mystring TransformationFile	= ini.read("transformation", mystring());

	if (not IO::file_exists(TransformationFile.c_str())) {
		generate_template();
		ini.print_help();
	}

	cmdline::msg(" - Loading transformations");
	// rotation matrices are set up
	read_transformations(TransformationFile.c_str());
	cmdline::ok();

	cmdline::msg(" - Loading data");

	mystring VecFile = input;

	if (not VecFile.empty() and not mylibs::IO::file_exists(VecFile.c_str()))
		cmdline::exit(VecFile + " does not exist !");

	mystring extension = VecFile.file_ext().lower();
	if (output.empty())
		output=VecFile.file_base()+"_transformed." + extension;
	else
		output = output.file_base() + ".vect";

	vector<Point> vec_ctrs;
	vector<Point> vec_dirs;
	if (extension == "vec" or extension == "vect"){
		// read in data
		SurfaceMesh::read_vec(vec_ctrs, vec_dirs, VecFile.c_str());
		cmdline::ok();

		cmdline::msg(" - Applying transformation");
		Point *p = &(vec_ctrs[0]);
		Point *d = &(vec_dirs[0]);
		for (vector<Transformation>::iterator it = Transformations.begin(); it != Transformations.end(); it++){
			for (size_t i = 0; i < vec_ctrs.size(); i++){
				it->apply(p[i]);
				it->apply_rotations(d[i]);
			}
		}
		cmdline::ok();

	} else if (extension == "pts" or extension == "vpts"){
		// read in data
		SurfaceMesh::read_pts(vec_ctrs, VecFile.c_str());
		cout << vec_ctrs.size() << " vectors read." << endl;
		cmdline::ok();

		cmdline::msg(" - Applying transformation");
		Point *p = &(vec_ctrs[0]);
		for (vector<Transformation>::iterator it = Transformations.begin(); it != Transformations.end(); it++){
			for (size_t i = 0; i < vec_ctrs.size(); i++)
				it->apply(p[i]);
		}
		cmdline::ok();
	} else {
		cerr << "Error: File extension '"			<< extension
			 <<"' of input data is unknown ! Exit." 	<< endl;
		exit(-1);
	}

	if (debug){
		cmdline::section("Results");

		if (vec_ctrs.size() == vec_dirs.size() and vec_ctrs.size() > 0){
			Point *p = &(vec_ctrs[0]);
			Point *d = &(vec_dirs[0]);
			for (size_t i = 0; i < vec_ctrs.size(); i++){
				cout << i << " : " << p[i] << " " << d[i] << endl;
			}
		} else {
			Point *p = &(vec_ctrs[0]);
			for (size_t i = 0; i < vec_ctrs.size(); i++){
				cout << i << " : " << p[i] << endl;
			}
		}
		cmdline::ok();
	}

	cmdline::section("Writing output");
	{
		if (vec_ctrs.size() == vec_dirs.size() and vec_ctrs.size() > 0){
			cmdline::msg(" - Writing " + output);
			Point *p = &(vec_ctrs[0]);
			Point *d = &(vec_dirs[0]);
			ofstream f(output);
			f << vec_ctrs.size() << endl;
			for (size_t i = 0; i < vec_ctrs.size(); i++){
				f << p[i] << " " << d[i] << endl;
			}
			f.close();
		}
	}

	{ // save output to file
		mystring ou = output.file_base()+".vpts";
		cmdline::msg(" - Writing " + ou);
		ofstream f(ou);
		f << vec_ctrs.size() << endl;
		Point *p = &(vec_ctrs[0]);
		for (size_t i = 0; i < vec_ctrs.size(); i++){
			f << p[i] << endl;
		}
		f.close();
	}
	cmdline::ok();

	return 0;
}
